/*
 * speed_control.c
 *
 *  Created on: Nov 20, 2024
 *      Author: Vinson
 */
#include "speed_control.h"
#include "sensor.h"
#include "math.h"
#include "stdbool.h"
#include "micros.h"
#include "utils.h"
#include "lowpass_filter.h"
#include "sample_current.h"
#include "current_control.h"

extern gpio_t dbg_out2;

static MC_Type mc_type;

int print_cnt        = PRINT_LOOP_NUMBER;
//static float zero_electric_angle = 0.485313 ;     // 电机1角度 0度偏移
//static float zero_electric_angle = 2.672580;
//static float zero_electric_angle = 5.083614;
//static float zero_electric_angle =  0.414176;
static float zero_electric_angle =  0;
static float sensor_offset = 0; 				 // sensor 偏移
static int sensor_direction = 0;
static int pole_pairs = U_MOTOR_PP;

/**
 * @brief  ：角度归一化[0~2PI]
 * @name   ：_normalizeAngle
 * @param  ：angle, 角度值
 * @return ：返回值，返回角度。
 * @details：将角度转化为弧度。
 */
static double _normalizeAngle(double angle)
{
	/* 取余数 */
	double a = fmod(angle, 2 * PI);
	return a >= 0 ? a : (a + 2 * PI);
}

/**
 * @brief  ：机械角度到电角度
 * @name   ：_electricalAngle
 * @param  ：无
 * @return ： 返回值，返回电角度。
 * @details：根据机械角度，得到电角度。
 */
static float electricalAngle(){
  return  _normalizeAngle( (float)(sensor_direction * U_MOTOR_PP) * getMechanicalAngle()  - sensor_direction * zero_electric_angle );
}

/**
 * @brief：设置ABC 三相pwm
 * @name：setPwm。
 * @param：Ua、Ub、Uc ABC 三相电压
 * @return： 无。
 * @details：设置ABC 三相电压占空比，配置PWM 输出。
 */
static void setPwm(float Ua, float Ub, float Uc)
{

    /* Step1: 限制ABC 三相上限 */
	Ua = clip(Ua, 0.0f, VDD_VAL);
	Ub = clip(Ub, 0.0f, VDD_VAL);
	Uc = clip(Uc, 0.0f, VDD_VAL);

	/* Step2: 限制占空比从0到1, 仅测量 两相adc值需要降低PWM输出 到 90% */
	float dc_a = 1.0f - clip(Ua / VDD_VAL, 0.0f, 0.9f);
	float dc_b = 1.0f - clip(Ub / VDD_VAL, 0.0f, 0.9f);
	float dc_c = 1.0f - clip(Uc / VDD_VAL, 0.0f, 0.9f);

	//if (print_cnt-- == 0) {
          /* 用于上位机波形显示，打印开启会降低电机转速 */
        //  printf("UaUbUc: %.2f,%.2d,%.2f\n", Ua, Ub, Uc);
        //  print_cnt = PRINT_LOOP_NUMBER;
    // }

	/* Step3: 写入 PWM 到 PWM 0-1-2 通道, pwm range: [1, 4095] */
	SETA_PHASE_PWM(dc_a);
	if (sensor_direction == DIR_CCW) {
		SETB_PHASE_PWM(dc_b);
		SETC_PHASE_PWM(dc_c);
	} else {
		SETC_PHASE_PWM(dc_b);
		SETB_PHASE_PWM(dc_c);
	}
}

/**
 * @brief  ：设置力矩。
 * @name   ：setTorque
 * @param  ：Uq q轴电压
 * @param  ：Ud d轴电压
 * @param  ：angle_el 电角度
 * @return ： 返回值。
 * @details：更详细的函数描述。
 */
static void setTorque(float Uq,float Ud, float angle_el)
{
	float Ualpha, Ubeta, Ua, Ub, Uc;

	//angle_el = _normalizeAngle(angle_el);

	/* Step3：帕克逆变换 */
	Ualpha =  -Uq * sin(angle_el);
	Ubeta =   Uq * cos(angle_el);

	/* Step4：克拉克逆变换 */
	//gpio_set_value(&dbg_out2, 0);
	Ua = Ualpha + VDD_VAL / 2;
	Ub = (_SQRT3 * Ubeta - Ualpha) / 2 + VDD_VAL / 2;
	Uc = (-Ualpha - _SQRT3 * Ubeta) / 2 + VDD_VAL / 2;
	//gpio_set_value(&dbg_out2, 1);
	setPwm(Ua, Ub , Uc);
}

int alignSensor()
{
	float voltage_align = 1.0f;

	/*Step1：电机方向判断 */
	if (sensor_direction == DIR_UNKNOWN) {
		/* Step1.1：向前移动 */
		for (int i = 0; i < 500; i++) {
			float angle = _3PI_2 + _2PI * i / 500.0f;
			setTorque(voltage_align, 0,  angle);
			sensor_update();
			delay_us(2 * 1000);
		}

		/* Step1.2：获取转动后的角度 */
		sensor_update();
		float mid_angle = getFullAngle();

		/* Step1.3：向后移动 */
		for (int i = 500; i >=0; i-- ) {
			float angle = _3PI_2 + _2PI * i / 500.0f ;
			setTorque(voltage_align, 0,  angle);
			sensor_update();
			delay_us(2 * 1000);
		}

		/* Step1.4：获取转动后的角度 */
		sensor_update();
		float end_angle = getFullAngle();

		delay_us(200 * 1000);

		float moved =  fabs(mid_angle - end_angle);
		if (moved < MIN_ANGLE_DETECT_MOVEMENT) {
			printf("电机移动失败\n");
			return 0;
		} else if (mid_angle < end_angle) {
			sensor_direction = DIR_CCW;
			printf("DIR_CCW \n");
		} else {
			sensor_direction = DIR_CW;
			printf("DIR_CW\n");
		}

		/* Step2：检查极对数. 0.5 是一个任意值，它可以是更高或者更低 */
		bool pp_check_result = (fabs(moved * pole_pairs - _2PI) < 0.5f);
		if( pp_check_result == false ) {
			printf("MOT: PP check: fail - estimated pp: %f \n", _2PI /moved);
		} else {
			printf("MOT: PP check: OK! \n");
		}

	}

	/*Step3：zero_electric_angle 校准 */
	if(zero_electric_angle == 0) {
		/* Step3.1 ：set angle -90(270 = 3PI/2) degrees */
		setTorque(voltage_align, 0,  _3PI_2);
		delay_us(700 * 1000);
		sensor_update();

		/* Step3.2 获取当前 zero electric angle 值 */
		zero_electric_angle = 0;
		zero_electric_angle = electricalAngle();
		printf("zero_electric_angle = %f \n",zero_electric_angle);

		//if (zero_electric_angle > PI) {
		//	zero_electric_angle = _2PI - zero_electric_angle;
		//	zero_electric_angle = -zero_electric_angle;
		//	printf("!zero_electric_angle = %f \n",zero_electric_angle);
		//}

		delay_us(20 * 1000);

		/* Step3.3 停止运动 */
		setTorque(0, 0, 0);
		delay_us(200 * 1000);
		sensor_update();
	}

	return 1;
}

float shaftAngle()
{
	float lpf_angle = m0_lpf_angle->lpf(m0_lpf_angle,getFullAngle());
	return sensor_direction * lpf_angle - sensor_offset;
}

// shaft velocity calculation
float shaftVelocity() {
	float lpf_vel = m0_lpf_vel->lpf(m0_lpf_vel, getVelocity());
	return sensor_direction * lpf_vel;
}


/* 位置环控制 */

/*
 * 角度pid：参数为角度[0~360]
 **/
float angle_loop_pid_get(float input)
{
	return m0_anglePid->pid_control(m0_anglePid, input);
}

/* 速度环控制 */

float speed_loop_pid_get(float input)
{
	return m0_speedPid->pid_control(m0_speedPid, input);
}

/*---------电流控制 ----------------- */
static void current_update(float *ia, float *ib)
{
	return adc_get_phaseCurrent(ia, ib);
}
static float shaftCurrent(float ia, float ib, float angle)
{
	float lpf_cur = m0_lpf_current->lpf(m0_lpf_current, getDccurrent(ia, ib, angle));
	return lpf_cur;
}

static float current_loop_pid_get(float input)
{
	return m0_currentPid->pid_control(m0_currentPid, input);
}

static void current_loop_pid_setParams(float p, float i, float d, float ramp, float limit)
{
	m0_currentPid->set(m0_currentPid, p, i, d, ramp, limit);
}

static void angle_pid_setParams(float p, float i, float d, float ramp, float limit)
{
	m0_anglePid->set(m0_anglePid, p, i, d, ramp, limit);
}

/* 速度环 */
void motor_setVelocity(float target_speed)
{
	float Uq = 0, Ud = 0;
	float err;

	float cur_speed = shaftVelocity();


	err = target_speed - cur_speed;
	/* 速度环 */
	Uq = 1.0 * speed_loop_pid_get(err * 180 / PI);
	setTorque(Uq, Ud, electricalAngle());


	{
	  /* 用于上位机波形显示，打印开启会降低电机转速 */
		static int vel_cnt = 0;
		MC_DEBUG_VEL(vel_cnt,"%f,%f,%f\n", target_speed, cur_speed, electricalAngle());
		if (vel_cnt++ == PRINT_LOOP_NUMBER)
			vel_cnt = 0;
	}
}

void motor_setAngle(float target_angle)
{
	float Uq = 0, Ud = 0;
	float err;
	float cur_angle = shaftAngle();

	/* 角度PID : 角度 = 弧度 * 180 / PI
	 * PI 环
	 **/
	err = (target_angle - cur_angle) * 180 / PI;
	Uq = 1.0 * angle_loop_pid_get(err);

	setTorque(Uq, Ud, electricalAngle());
	{
		static int angle_cnt = 0;
		MC_DEBUG_VEL(angle_cnt, "%f,%f \n", target_angle, cur_angle);
		if (angle_cnt++ == PRINT_LOOP_NUMBER)
			angle_cnt = 0;
	}
}

/* 电流环控制 */
void motor_setCurrent(float target, float ia, float ib)
{
	//gpio_set_value(&dbg_out2, 0);
	float current = shaftCurrent(ia, ib, electricalAngle());
	//gpio_set_value(&dbg_out2, 1);
	float err = 0;
	err = target - current;
	float Uq = 0;
	Uq = current_loop_pid_get(err);
	setTorque(Uq, 0, electricalAngle());

	{
	  /* 用于上位机波形显示，打印开启会降低电机转速 */
		static int current_cnt = 0;
		MC_DEBUG_VEL(current_cnt, "%f,%f,%f\n", target, current, Uq);
		if (current_cnt++ == PRINT_LOOP_NUMBER)
			current_cnt = 0;
	}
}

/*
 * 角度-电流
 */
void motor_setAngleCurrent(float target_angle, float ia, float ib)
{
	float Uq = 0;
	float err;
	float cur_angle = shaftAngle();

	/* 角度PID : 角度 = 弧度 * 180 / PI
	 * PI 环
	 **/
	err = (target_angle - cur_angle) * 180 / PI;
	float current_iq = angle_loop_pid_get(err);

	/*
	 * 电流环控制：
	 */
	motor_setCurrent(current_iq, ia, ib);
}

/*
 * 速度-电流
 */
void motor_setVelCurrent(float target_speed, float ia, float ib)
{
	float err;

	float cur_speed = shaftVelocity();


	err = target_speed - cur_speed;
	/* 速度环 */
	float current_iq = 1.0 * speed_loop_pid_get(err * 180 / PI);

	/*
	 * 电流环控制：
	 */
	motor_setCurrent(current_iq, ia, ib);

}

/*
 * 位置-速度闭环：弧度
 */
void motor_setAngleVeL(float target_angle)
{
	float Uq = 0, Ud = 0;
	float err;
	float cur_angle = shaftAngle();

	/* 角度PID : 角度 = 弧度 * 180 / PI
	 * PI 环
	 **/
	err = (target_angle - cur_angle) * 180 / PI;
	err = angle_loop_pid_get(err);
	float lpf_speed = err;

	/*
	 * 获取速度考虑滤波
	 */
	float cur_speed = shaftVelocity();

	/* 速度PID：*/
	Uq = speed_loop_pid_get(err - cur_speed);

	setTorque(Uq, Ud, electricalAngle());
	{
		static int angle_vel_cnt = 0;
		MC_DEBUG_VEL(angle_vel_cnt, "%f,%f,%f,%f\n", target_angle, cur_angle, lpf_speed, cur_speed);
		if (angle_vel_cnt++ == PRINT_LOOP_NUMBER)
			angle_vel_cnt = 0;
	}

}

int initFOC()
{
	int exit_flag = 0;
	/* Step0: ADC 采样电流初始化 */

	adc_current_init();
	/* Step1: 编码器校准 */
	exit_flag = alignSensor();

	if (!exit_flag)
		return 0;

	/* 设置ADC 采样相数，三相采两相 */
	//if (sensor_direction == DIR_CW) {
	//	adc_current_dir_set(ADC_GET_BC);
	//} else {
	//	adc_current_dir_set(ADC_GET_AC);
	//}

	/*Step2: 低通滤波器初始化 */
	m0_lpf_angle = lowpass_filter_create(0.01);		/* 角度 */
	m0_lpf_vel = lowpass_filter_create(0.01);		/* 速度 */
	m0_lpf_current = lowpass_filter_create(0.08);	/* 电流 */

	/*Step3: PID 控制器初始化 */
	float p = 0.005f;
	float i = 0.01f;
	float d = 0;
	float ramp = 0;
	float limit = VDD_VAL;
	m0_speedPid = pid_control_create(p, i, d, ramp, limit); /* 速度 PI 环 */
	m0_currentPid = pid_control_create(p, i, d, ramp, limit); /* 电流 PI 环 */
	m0_anglePid = pid_control_create(p, i, d, ramp, limit);  /* 角度 PI 环 */

	current_loop_pid_setParams(5, 100, 0, 100000, limit);

	angle_pid_setParams(1, 0, 0, 100000, 100);

	mc_type = Type_velocity_current;

	isReadyOn = 1;
	return 1;
}

/* @brief  ：FoC 控制逻辑
 * @name   ：runFoC
 * @param  ：无
 * @return ：无
 * @details：无
 */
void runFoC()
{
	if (!isReadyOn)
		return ;

	sensor_update();
	switch (mc_type) {

	/* 位置闭环 ok */
	case Type_angle:
		motor_setAngle(TARGET_ANGEL);
			break;
	/* 速度闭环 ok */
	case Type_velocity:
		motor_setVelocity(TARGET_VEL);
		break;

	/* 电流闭环 */
	case Type_current:
		{
			float ia, ib;
			current_update(&ia, &ib);
			motor_setCurrent(TARGET_IQ, ia, ib);
		}
		break;

	/* 角度-电流 */
	case Type_angle_current:
		{
			float ia, ib;
			current_update(&ia, &ib);
			//motor_setAngleCurrent(TARGET_ANGEL, ia, ib);
		}
		break;
	/* 速度-电流*/
	case Type_velocity_current:
		{
			float ia, ib;
			current_update(&ia, &ib);
			motor_setVelCurrent(TARGET_VEL, ia, ib);
		}
		break;

	/* 位置-速度控制：ok */
	case Type_angle_velocity:
		motor_setAngleVeL(TARGET_ANGEL);
		break;

	/* 速度开环 */
	case Type_velocity_openloop:
		break;

	/* 角度开环 */
	case Type_angle_openloop:
		break;

	}

#if 0
	/*Step1：更新编码器的值 */
	float ia, ib;
	//gpio_set_value(&dbg_out2, 0);
	sensor_update();
	//gpio_set_value(&dbg_out2, 1);
	current_update(&ia, &ib);

	motor_setIq(0.2, ia, ib);

	//motor_setSpeed(150);
#endif

}
